#ifndef LB_Quaternions_H_
#define LB_Quaternions_H_

#include<Math/LB_Quaternions.h>
#include<Math/LB_Math.h>

#define SIGN(x) ((x>0)?1:((x<0)?-1:0))
#ifndef SAFE_SQRT
#define SAFE_SQRT(x) (x>=0?sqrt(x):0)
#endif
#ifndef SAFE_FREE
#define SAFE_FREE(x) if(x!=NULL){free(x);x=NULL;}
#endif


Quaternions quaternions_mult(Quaternions q1,Quaternions q2)
{
    Quaternions re;
    quaternions_init(&re);

    re.v[0]=q1.v[0]*q2.v[0]-q1.v[1]*q2.v[1]- q1.v[2]*q2.v[2]- q1.v[3]*q2.v[3];

    re.v[1]=q1.v[0]*q2.v[1]+ q1.v[1]*q2.v[0] + q1.v[2]*q2.v[3]- q1.v[3]*q2.v[2];

    re.v[2]=q1.v[0]*q2.v[2]+q1.v[2]*q2.v[0]+ q1.v[3]*q2.v[1]- q1.v[1]*q2.v[3];

    re.v[3]=q1.v[0]*q2.v[3]+q1.v[3]*q2.v[0]+ q1.v[1]*q2.v[2]- q1.v[2]*q2.v[1];

    return re;
}


Quaternions quaternions_plus(Quaternions q1,Quaternions q2)
{
    Quaternions re;
    quaternions_init(&re);

    re.v[0]=q1.v[0]+q1.v[0];
    re.v[1]=q1.v[1]+q1.v[1];
    re.v[2]=q1.v[2]+q1.v[2];
    re.v[3]=q1.v[3]+q1.v[3];
    return re;
}
Quaternions q_rota(Quaternions q1,double* x)
{
    Quaternions re;
    re.v[0]=0;
    re.v[1]=x[0]; re.v[2]=x[1]; re.v[3]=x[2];

    Quaternions q_ =quaternions_inverse(q1);

    return quaternions_mult(q1,quaternions_mult(re,q_));
    
}

Quaternions quaternions_conjugate(Quaternions q )
{
    Quaternions re;
    quaternions_init(&re);
    re.v[0]=q.v[0];
    re.v[1]=-q.v[1];re.v[2]=-q.v[2];re.v[3]=-q.v[3];
    return re;
}

double quaternions_norm(Quaternions q )
{
    return SAFE_SQRT(q.v[0]*q.v[0]+q.v[1]*q.v[1]+q.v[2]*q.v[2]+q.v[3]*q.v[3]);
}


Quaternions quaternions_inverse(Quaternions q )
{
    double norm= quaternions_norm(q);

    Quaternions re;
    quaternions_init(&re);

    re.v[0]=q.v[0]/norm;
    re.v[1]=-q.v[1]/norm;re.v[2]=-q.v[2]/norm;re.v[3]=-q.v[3]/norm;

    return re;
}

double* get_rotation_from_matrix(double* matrix)
{
    double* re=(double*)malloc(sizeof(double)*4);
    memset(re,0,sizeof(double)*4);
    
    re[0]=acos( (matrix[0]+matrix[4]+matrix[8]  -1)/2  );
    if(fabs(re[0])<1e-8)
    {
        re[0]=0;
        re[1]=1.0;
        return re;
    } 
    re[1]=(matrix[7]-matrix[5] )/(2*sin(re[0]));
    re[2]=(matrix[2]-matrix[6] )/(2*sin(re[0]));
    re[3]=(matrix[3]-matrix[1] )/(2*sin(re[0]));
    return re; 
}

double*  get_rotation_from_quaternions( Quaternions q  )
{
    double* re=(double*)malloc(sizeof(double)*4);
    re[0]=2*acos( q.v[0]);
    re[1]=q.v[1];re[2]=q.v[2];re[3]=q.v[3];
    normalize(&(re[1]),3);
    return re;
}

double*  get_matrix_from_quaternions( Quaternions q  )
{
    double theta=  2*acos( q.v[0]);
    double ax[3]={q.v[1],q.v[2],q.v[3]}; 
    normalize(ax,3);
    return get_matrix_from_rotation(theta,ax); 
}


#endif